
#include <asm.h>

.global C_SYMBOL(RECT888To666RCWWMMX)

C_SYMBOL(RECT888To666RCWWMMX):

zWidth= -0x2C
yDstPt= -0x28
ySrcPt= -0x24
yWidth= -0x20
y= -0x1C
height = -0x18
width = -0x14
DstPitch= -0x10
SrcPitch= -0xC
DstPt= -0x8
SrcPt= -0x4
//  .long 0xe7fddefe

    MOV     R12, SP
    STMFD	SP!, {R4-R12,LR}
    SUB     SP, SP, #0x40

    LDR R11, [R0, #0x0]                 //pSrc
    LDR	R8,  [R0, #0xC]                 //SrcPitch
    LDR	R10, [R1, #0x0]                 //pDst
	LDR	R12, [R1, #0xC]                 //DstPitch

    LDR	R4, [R0, #0x4]                  //width
	LDR	R7, [R0, #0x8]                  //height
	STR R4, [SP, #0x40+width]
	STR R4, [SP, #0x40+yWidth]
	STR R7, [SP, #0x40+height]
	STR R8, [SP, #0x40+SrcPitch]
	STR R12, [SP, #0x40+DstPitch]

    SUB R6, R7, #1
	MLA R11, R6, R8, R11
	STR R11, [SP, #0x40+SrcPt]
	STR R10, [SP, #0x40+DstPt]

    MOV     R5, #0

    LDR     R4, = 0x00000002
    TMCR    wCGR1, R4
    LDR     R4, = 0x00000004
    TMCR    wCGR2, R4
    LDR     R4, = 0x00000006
    TMCR    wCGR3, R4
    LDR     R4, = 0x000000FC
    TMCRR   wR13, R4 , R4
    LDR     R4, = 0x0000FC00
    TMCRR   wR14, R4 , R4
    LDR     R4, = 0x00FC0000
    TMCRR   wR15, R4 , R4
    WZERO   wR1

	LDR R0, [SP, #0x40+SrcPt]
	LDR R2, [SP, #0x40+DstPt]
//.long 0xe7fddefe

    ADD	R6, R2,	#7
    BIC	R6, R6,	#7
    SUB	R6, R6,	R2
    MOV R6, R6, LSR #2
    CMP	R6, #0

    BEQ	_Aligned8_

    LDR R7, [SP, #0x40+height]
    CMP	    R6, R7
    MOVGT	R6, R7
    SUB R7, R7, R6
    STR R6, [SP, #0x40+y]
    STR R7, [SP, #0x40+height]

    BL _Process_One_Line_

_Aligned8_:
    LDR R6, [SP, #0x40+width]
    LDR R7, [SP, #0x40+height]
    CMP R7, #0
    BLE End

    SUBS R7, R7, #4
    STRGE R7, [SP, #0x40+height]
    BGE _Skip_Line_

    ADD R7, R7, #4
    STR R7, [SP, #0x40+y]
    BL	_Process_One_Line_
    B End

_Skip_Line_:

    AND	R4, R0,	#7
    CMP R4, #0

    LDR     R8, [SP, #0x40+SrcPitch]
    MOV     R4, R2
    SUB     R1, R0, R8
    SUB     R2, R1, R8
    SUB     R3, R2, R8

    BEQ _Skip_Head_

    MOV R9, #1
    STR R9, [SP, #0x40+yWidth]

    BL _Process_One_Block_
    LDR R6, [SP, #0x40+width]
    SUBS R6, R6, #1
    BLE _Aligned8_

_Skip_Head_:

    SUBS R6, R6, #4
    BGE _Skip_Tail_
    ADD R6, R6, #4
    STR R6, [SP, #0x40+yWidth]
    BL _Process_One_Block_

    B _Aligned8_

_Skip_Tail_:

    WLDRD   wR9, [R0], #8       @pData
    WLDRD   wR10, [R1], #8       @pData
    WLDRD   wR11, [R2], #8       @pData
    WLDRD   wR12, [R3], #8       @pData

Loop4:

    SUBS    R6, R6, #4

    PLD	    [R0,#0x40]
    PLD	    [R1,#0x40]
    PLD	    [R2,#0x40]
    PLD	    [R3,#0x40]

    WUNPCKILW wR7, wR9, wR10
    WUNPCKILW wR8, wR11, wR12

    WAND    wR0, wR7, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR7, wR14          //G
    WSRLWG  wR1, wR1, wCGR2

    WAND    wR2, wR7, wR15          //R
    WSRLWG  wR2, wR2, wCGR3

    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WAND    wR4, wR8, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR1, wR8, wR14          //G
    WSRLWG  wR1, wR1, wCGR2

    WAND    wR2, wR8, wR15          //R
    WSRLWG  wR2, wR2, wCGR3

    WOR     wR4, wR4, wR1
    WOR     wR4, wR4, wR2

    WSTRD   wR0, [R4]
    WSTRD   wR4, [R4, #8]

    ADD     R4, R4,  R12

    WUNPCKIHW wR7, wR9, wR10
    WUNPCKIHW wR8, wR11, wR12

    WAND    wR0, wR7, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR7, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRD   wR9, [R0], #8
    WAND    wR2, wR7, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRD   wR10, [R1], #8
    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WAND    wR4, wR8, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR1, wR8, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRD   wR11, [R2], #8
    WAND    wR2, wR8, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRD   wR12, [R3], #8
    WOR     wR4, wR4, wR1
    WOR     wR4, wR4, wR2

    WSTRD   wR0, [R4]
    WSTRD   wR4, [R4,#8]

    ADD     R4, R4,  R12

//////////// Round 2///////////////////

    WUNPCKILW wR7, wR9, wR10
    WUNPCKILW wR8, wR11, wR12

    WAND    wR0, wR7, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR7, wR14          //G
    WSRLWG  wR1, wR1, wCGR2

    WAND    wR2, wR7, wR15          //R
    WSRLWG  wR2, wR2, wCGR3

    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WAND    wR4, wR8, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR1, wR8, wR14          //G
    WSRLWG  wR1, wR1, wCGR2

    WAND    wR2, wR8, wR15          //R
    WSRLWG  wR2, wR2, wCGR3

    WOR     wR4, wR4, wR1
    WOR     wR4, wR4, wR2

    WSTRD   wR0, [R4]
    WSTRD   wR4, [R4, #8]
    ADD     R4, R4,  R12

    WUNPCKIHW wR7, wR9, wR10
    WUNPCKIHW wR8, wR11, wR12

    WAND    wR0, wR7, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR7, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRDGE   wR9, [R0], #8
    WAND    wR2, wR7, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRDGE   wR10, [R1], #8
    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WAND    wR4, wR8, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR1, wR8, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRDGE   wR11, [R2], #8
    WAND    wR2, wR8, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRDGE   wR12, [R3], #8
    WOR     wR4, wR4, wR1
    WOR     wR4, wR4, wR2

    WSTRD   wR0, [R4]
    WSTRD   wR4, [R4,#8]

    ADD     R4, R4,  R12

    BGE     Loop4

    ADDS	R6, R6,	#4
    BEQ     _Skip_

    STR R6, [SP, #0x40+yWidth]

    BLGT	_Process_One_Block_
_Skip_:
    LDR     R8, [SP, #0x40+SrcPitch]
    LDR     R0, [SP, #0x40+SrcPt]
    MOV  R9, #-4
    MLA  R0, R8, R9, R0
    LDR  R2, [SP, #0x40+DstPt]
    ADD  R2, R2, #16
    STR  R2, [SP, #0x40+DstPt]
    STR  R0, [SP, #0x40+SrcPt]
    B _Aligned8_


End:
    ADD	SP, SP,	#0x40
    LDMFD   SP, {R4-R11,SP,PC}

_Process_One_Line_:

    LDR R6, [SP, #0x40+width]
    WLDRW   wR4, [R0], #4

_Loop_One_:

    SUBS    R6, R6,	#1

    WAND    wR0, wR4, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR4, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
    WAND    wR2, wR4, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
    WLDRWGT wR4, [R0], #4

    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WSTRW   wR0, [R2]

    ADD     R2, R2, R12

    BGT	_Loop_One_

    LDR     R8, [SP, #0x40+SrcPitch]                //SrcPitch
    LDR     R0, [SP, #0x40+SrcPt]
    LDR     R2, [SP, #0x40+DstPt]
    LDR     R7, [SP, #0x40+y]
    SUB     R0, R0, R8
    ADD     R2, R2, #4
    SUBS    R7, R7, #1
    STR     R7, [SP, #0x40+y]
    STR     R2, [SP, #0x40+DstPt]
    STR     R0, [SP, #0x40+SrcPt]

    BGT     _Process_One_Line_
    MOV     PC, LR

_Process_One_Block_:

    LDR R6, [SP, #0x40+yWidth]
    WLDRW   wR9,  [R0], #4       @pData
    WLDRW   wR10, [R1], #4       @pData
    WLDRW   wR11, [R2], #4       @pData
    WLDRW   wR12, [R3], #4       @pData

_Loop_One_Block_:

    SUBS R6, R6, #1

    WUNPCKILW wR2, wR9, wR10
    WUNPCKILW wR3, wR11, wR12

    WAND    wR0, wR2, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR2, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRWGT   wR9, [R0], #4
    WAND    wR2, wR2, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRWGT   wR10, [R1], #4
    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2


    WAND    wR4, wR3, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR1, wR3, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
        WLDRWGT   wR11, [R2], #4
    WAND    wR2, wR3, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
        WLDRWGT   wR12, [R3], #4
    WOR     wR4, wR4, wR1
    WOR     wR4, wR4, wR2

    WSTRD   wR0, [R4]
    WSTRD   wR4, [R4, #8]

    ADD     R4, R4,  R12

    BGT	_Loop_One_Block_

    MOV  PC, LR
